/*
 * top.c
 *
 *  Created on: 06/11/2012
 *      Author: julian
 */

#include <sys/ipc.h>
#include <sys/msg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "app.h"
#include "message.h"
#include "msgq_utils.h"
#include "posix_mq_utils.h"
#include "serializationUtils.h"

static int txs_msqid;
static int at_msqid;
static int atr_msqid;

wpan_node_t nodes;
static uint8_t _num_nodes;
static pthread_mutex_t node_list_mutex;

static void print_node_info(wpan_node_t *tmp) {
    printf("\naddr16: %#X ", tmp->adress16);
    printf("addr64: %#X", tmp->adress64 >> 32);
    printf("%X ", tmp->adress64);
    printf("NI: %s ", tmp->identifier);
    printf("msqid: %d ", tmp->msqid);
    printf("msq_name: %s \n", tmp->msq_name);
}

int getTXSMsgq_id() {
    return txs_msqid;
}

int getNumNodes() {
    return _num_nodes;
}

void setNumNodes(uint8_t val) {
    _num_nodes = val;
}

wpan_node_t * getNodeList() {
    return &nodes;
}

int sendATRemoteCommand(char *command, uint16_t addr16, uint64_t addr64,
        long long val, char *strval, uint8_t fid) {

    if (command[0] == '0' || command == NULL || strlen(command)!=2) {
        return -1;
    }

    xbee_AtRemote_t frame;
    frame.destinationAddress64 = addr64;
    frame.destinationAddress16 = addr16; //0xfffe if you want to ignore it, otherwise it WILL be used
    frame.frameId = fid;
    frame.options = 0x02;
    frame.command[0] = command[0];
    frame.command[1] = command[1];

    if (val >= 0) {
        frame.valueSize = sizeof(val);
        frame.value = malloc(frame.valueSize * sizeof(uint8_t));
        memset(frame.value, 0, frame.valueSize);
        serializeLong(frame.value, val);

#ifdef APP_LAYER_DEBUG
        uint8_t i = 0;
        printf("\nparam: ");
        for (i = 0; i < frame.valueSize; i++) {
            printf("%X ", frame.value[i]);
        }
        printf("\n");
#endif

        if (packetOutput(&frame, AT_COMMAND_REMOTE_REQUEST) == -1) {
#ifdef APP_LAYER_DEBUG
            fprintf(stderr,"sendATRemoteCommand:packetOutput.Error al enviar trama");
#endif
            return -1;
        }

        frame.valueSize = 0;
        frame.command[0] = 'A';
        frame.command[1] = 'C';

        if (packetOutput(&frame, AT_COMMAND_REMOTE_REQUEST) == -1) {
#ifdef APP_LAYER_DEBUG
            fprintf(stderr,"sendATRemoteCommand:packetOutput.Error al enviar trama");
#endif
            return -1;
        }

//        frame.command[0] = 'W';
//        frame.command[1] = 'R';
//
//        if (packetOutput(&frame, AT_COMMAND_REMOTE_REQUEST) == -1) {
//#ifdef APP_LAYER_DEBUG
//            perror("sendATRemoteCommand:packetOutput");
//#endif
//            return -1;
//        }

        free(frame.value);
        return 0;
    } else if (strval != NULL) {

        frame.valueSize = strlen(strval);
        frame.value = malloc(frame.valueSize * sizeof(uint8_t));
        memset(frame.value, 0, frame.valueSize);

        strcpy(frame.value, strval);

#ifdef APP_LAYER_DEBUG
        printf("\nparam: %s\n", frame.value);
#endif

        if (packetOutput(&frame, AT_COMMAND_REMOTE_REQUEST) == -1) {
#ifdef APP_LAYER_DEBUG
            fprintf(stderr,"sendATRemoteCommand:packetOutput.Error al enviar trama");
#endif
            return -1;
        }

        frame.valueSize = 0;
        frame.command[0] = 'A';
        frame.command[1] = 'C';

        if (packetOutput(&frame, AT_COMMAND_REMOTE_REQUEST) == -1) {
#ifdef APP_LAYER_DEBUG
            fprintf(stderr,"sendATRemoteCommand:packetOutput.Error al enviar trama");
#endif
            return -1;
        }

        frame.command[0] = 'W';
        frame.command[1] = 'R';

        if (packetOutput(&frame, AT_COMMAND_REMOTE_REQUEST) == -1) {
#ifdef APP_LAYER_DEBUG
            fprintf(stderr,"sendATRemoteCommand:packetOutput.Error al enviar trama");
#endif
            return -1;
        }

        free(frame.value);
        return 0;
    } else {
        frame.valueSize = 0;

        if (packetOutput(&frame, AT_COMMAND_REMOTE_REQUEST) == -1) {
#ifdef APP_LAYER_DEBUG
            fprintf(stderr,"sendATRemoteCommand:packetOutput.Error al enviar trama");
#endif
            return -1;
        }
        return 0;
    }

    if (fid != 0) {
        struct ATR_qbuf atr;

        if (pmsgq_receive(atr_msqid, &atr, sizeof(atr), NULL, 0, 1) == -1) {
#ifdef APP_LAYER_DEBUG
            perror("sendATRemoteCommand:pmsgq_recv.Error al recibir respuesta");
#endif
            return -1;
        } else {
            if (strcmp(atr.response.command, command) == 0
                    && atr.response.frameId == fid) {
                if (atr.response.status == 0) {
                    return 0;
                }
            } else {
#ifdef APP_LAYER_DEBUG
                fprintf(stderr,"sendATRemoteCommand:Error al recibir respuesta");
#endif
                return -1;
            }
        }
    } else {
        return 0;
    }

}

int sendATCommand(char *command, long long val, char *strval,
        uint8_t fid) {


    if (command[0] == '0' || (strlen(command) != 2)) {
        return -1;
    }

    xbee_AtCommand_t frame;
    frame.frameId = fid;
    frame.command[0] = command[0];
    frame.command[1] = command[1];

    if (val >= 0) {
        frame.parameterSize = sizeof(val);
        frame.parameter = malloc(frame.parameterSize * sizeof(uint8_t));
        memset(frame.parameter, 0, frame.parameterSize);
        serializeLong(frame.parameter, val);

#ifdef APP_LAYER_DEBUG
        uint8_t i = 0;
        printf("\nparam: ");
        for (i = 0; i < frame.parameterSize; i++) {
            printf("%X ", frame.parameter[i]);
        }
        printf("\n");
#endif

        if (packetOutput(&frame, AT_COMMAND) == -1) {
#ifdef APP_LAYER_DEBUG
            fprintf(stderr,"sendATCommand:packetOutput.Error al enviar trama");
#endif
            return -1;
        }
        free(frame.parameter);
        return 0;
    } else if (strval != NULL) {

        frame.parameterSize = strlen(strval);
        frame.parameter = malloc(frame.parameterSize * sizeof(uint8_t));
        memset(frame.parameter, 0, frame.parameterSize);

        strcpy(frame.parameter, strval);

#ifdef APP_LAYER_DEBUG
        printf("\nparam: %s\n", frame.parameter);
#endif

        if (packetOutput(&frame, AT_COMMAND) == -1) {
#ifdef APP_LAYER_DEBUG
            fprintf(stderr,"sendATCommand:packetOutput.Error al enviar trama");
#endif
            return -1;
        }
        free(frame.parameter);
        return 0;
    } else {
        frame.parameterSize = 0;

        if (packetOutput(&frame, AT_COMMAND) == -1) {
#ifdef APP_LAYER_DEBUG
            fprintf(stderr,"sendATCommand:packetOutput.Error al enviar trama");
#endif
            return -1;
        }
        return 0;
    }

//    if (fid != 0) {
//        struct AT_qbuf atc;
//
//        if (pmsgq_receive(at_msqid, &atc, sizeof(atc), NULL, 900, 0) == -1) {
//#ifdef APP_LAYER_DEBUG
//            perror("sendATCommand:pmsgq_Recv.Error al recibir respuesta");
//#endif
//            return -1;
//        } else {
//            if (strcmp(atc.response.command, command) == 0
//                    && atc.response.frameId == fid) {
//                if (atc.response.status == 0) {
//                    return 0;
//                }
//            } else {
//                fprintf(stderr, "sendATCommand:Error al recibir respuesta");
//                return -1;
//            }
//        }
//    }
//    else {
//        return 0;
//    }

}

int queryATCommand (char *command, uint8_t fid, uint8_t *buf) {

    uint8_t i = 0;

    if (command[0] == '0' || (strlen(command) != 2)) {
        return -1;
    }

    xbee_AtCommand_t frame;
    frame.frameId = fid;
    frame.command[0] = command[0];
    frame.command[1] = command[1];
    frame.parameterSize = 0;

    if (packetOutput(&frame, AT_COMMAND) == -1) {
#ifdef APP_LAYER_DEBUG
        fprintf(stderr,"queryATCommand:packetOutput.Error al enviar trama");
#endif
        return -1;
    }

    if (fid != 0) {
        struct AT_qbuf atc;

        if (pmsgq_receive(at_msqid, &atc, sizeof(atc), NULL, 900, 0) == -1) {
#ifdef APP_LAYER_DEBUG
            perror("queryATCommand:pmsgq_recv.Error al recibir respuesta");
#endif
            return -1;
        } else {
            if (strcmp(atc.response.command, command) == 0
                    && atc.response.frameId == fid) {
                if (atc.response.status == 0) {

                    buf = malloc(atc.response.valueSize * sizeof(uint8_t));
#ifdef APP_LAYER_DEBUG
                    printf("\nATC response value: ");
#endif
                    for (i = 0; i < atc.response.valueSize; ++i) {
                        buf[i] = atc.response.value[i];
#ifdef APP_LAYER_DEBUG
                        printf("%X ", buf[i]);
#endif
                    }
#ifdef APP_LAYER_DEBUG
                    printf("\n");
#endif
                    return 0;
                }
            } else {
                fprintf(stderr, "queryATCommand:Error al recibir respuesta");
                return -1;
            }
        }
    }

    return 0;

}


int nodeDiscovery() {

    pthread_mutex_lock(&node_list_mutex);

    if (sendATCommand("ND", -1, NULL, 1) == -1) {
#ifdef APP_LAYER_DEBUG
        fprintf(stderr,"nodeDiscovery:sendATCommand.Error al enviar comando");
#endif
        pthread_mutex_unlock(&node_list_mutex);
        return -1;
    }

//    if (sendATCommand("NI", -1, "CORDINADOR", 0) == -1) {
//#ifdef APP_LAYER_DEBUG
//      fprintf(stderr,"nodeDiscovery:sendATCommand.Error al enviar comando");
//#endif
//        //return -1;
//    }

    wpan_node_t *tmp;
    struct list_head *pos, *q;
    unsigned int i = 0, j = 0;

    if (nodes.node_list.next != 0 && nodes.node_list.prev != 0) {
        list_for_each_safe(pos, q, &nodes.node_list) {
            tmp = list_entry(pos, wpan_node_t, node_list);
            pmsgq_destroy(tmp->msq_name, tmp->msqid);
            list_del(pos);
            free(tmp);
        }
#ifdef APP_LAYER_DEBUG
        printf("\nnodeDiscovery deleted list.");
#endif
    }

    INIT_LIST_HEAD(&nodes.node_list);

    struct AT_qbuf atc;

    do {

        if (pmsgq_receive(at_msqid, &atc, sizeof(atc), NULL, ND_TIMEOUT_MSEC,
                ND_TIMEOUT_SEC) == -1) {
#ifdef APP_LAYER_DEBUG
            perror("nodeDiscovery:pmsgq_recv.Error al recibir respuesta");
#endif
            //return -1;
            break;
        } else {

            if (strcmp(atc.response.command, "ND") == 0
                    && atc.response.valueSize == 0) {
                break;
            } else if (strcmp(atc.response.command, "ND") == 0) {
                /* Node Discover Response (API format - data is binary (except for NI)):
                 2 bytes for MY (Source Address) value
                 4 bytes for SH (Serial Number High) value
                 4 bytes for SL (Serial Number Low) value
                 1 byte for DB (Received Signal Strength) value
                 NULL-terminated string for NI (Node Identifier) value (max 20 bytes w/out NULL terminator)
                 * */
                tmp = (wpan_node_t *) malloc(sizeof(wpan_node_t));
                tmp->adress16 = deSerializeShort(atc.response.value);
                tmp->adress64 = deSerializeLong(atc.response.value+2);

                for (i = 11; i < atc.response.valueSize; i++) {
                    tmp->identifier[i - 11] = atc.response.value[i];
                }

                list_add(&(tmp->node_list), &(nodes.node_list));

//                if (sendATRemoteCommand("MY", tmp->adress16, tmp->adress64, j+1, NULL, j+1)<0) {
//                    fprintf(stderr,"nodeDiscovery:sendATRemoteCommand.Error al enviar comando");
//                }

                j++;
            } else {

                break;
            }

        }
    } while (1);

    printf("\n%d nodes.", j);

    list_for_each(pos, &nodes.node_list) {
        tmp = list_entry(pos, wpan_node_t, node_list);
        strcpy(tmp->msq_name, "/");
        tmp->msqid = pmsgq_create(strcat(tmp->msq_name, tmp->identifier), 1000,
                sizeof(struct msg_buf)); // cola para almacenar mensajes entrantes
//          tmp->msqid = pmsgq_create(strcat(tmp->msq_name, tmp->identifier), 1000,
//                sizeof(struct msg_t)); // cola para almacenar mensajes entrantes
    }

    pthread_mutex_unlock(&node_list_mutex);

//    uint8_t *buf;
//    if (queryATCommand("MY", 2, buf) < 0) {
//#ifdef APP_LAYER_DEBUG
//        fprintf(stderr,"nodeDiscovery:queryATCommand.Error al consultar parametro");
//#endif
//        //return -1;
//    }

    return j;
}

void printNodes() {

    uint8_t i = 0;
    wpan_node_t *tmp = NULL;
    struct list_head *pos;

    list_for_each(pos, &nodes.node_list) {
        tmp = list_entry(pos, wpan_node_t, node_list);
        printf("\nnode %d: ", ++i);
        print_node_info(tmp);
    }
}

int listNodes(char **list) {

    uint8_t i = 0;
    wpan_node_t *tmp = NULL;
    struct list_head *pos;

    if (list == NULL) {
        errno = EINVAL;
        return (-1);
    }

    list_for_each(pos, &nodes.node_list) {
        tmp = list_entry(pos, wpan_node_t, node_list);
        strcpy(list[i++], tmp->identifier);
    }

    return i;
}

wpan_node_t *findNode(wpan_node_t *list, uint16_t addr16, uint64_t addr64) {

    int f = 0;
    wpan_node_t *tmp = NULL;
    struct list_head *pos;

    pthread_mutex_lock(&node_list_mutex);

    if (addr16 != 0) {
        list_for_each(pos, &list->node_list) {
            tmp = list_entry(pos, wpan_node_t, node_list);
            if (tmp->adress16 == addr16) {
                f = 1;
                break;
            }
        }
    } else {
        list_for_each(pos, &list->node_list) {
            tmp = list_entry(pos, wpan_node_t, node_list);
            if (tmp->adress64 == addr64) {
                f = 1;
                break;
            }
        }
    }

    pthread_mutex_unlock(&node_list_mutex);

    if (f) {
        return tmp;
    } else {
        return NULL;
    }
}

wpan_node_t *findNodeById(char *id) {

    int f = 0;
    wpan_node_t *tmp = NULL;
    struct list_head *pos;

    pthread_mutex_lock(&node_list_mutex);

    list_for_each(pos, &nodes.node_list) {
        tmp = list_entry(pos, wpan_node_t, node_list);
        if (strcmp(tmp->identifier, id) == 0) {
            f = 1;
            break;
        }
    }
    pthread_mutex_unlock(&node_list_mutex);

    if (f) {
        return tmp;
    } else {
        return NULL;
    }
}

int appLayerInit() {

    if (xbeeSvcInit() == -1) {
#ifdef APP_LAYER_DEBUG
        perror("appLayerInit:xbeeSvcInit.Error al inicializar");
#endif
        return -1;
    }

    txs_msqid = pmsgq_create(APP_TX_STATUS_QUEUE_NAME, 1000,
            sizeof(struct TX_status_qbuf));
    if (txs_msqid == -1) {
#ifdef APP_LAYER_DEBUG
        perror("appLayerInit:msgget.Error al crear la cola de tx status");
#endif
        return -1;
    }

    at_msqid = pmsgq_create(APP_AT_QUEUE_NAME, 1000, sizeof(struct AT_qbuf));
    if (at_msqid == -1) {
#ifdef APP_LAYER_DEBUG
        perror("appLayerInit:msgget.Error al crear la cola de respuestas AT");
#endif
        return -1;
    }

    atr_msqid = pmsgq_create(APP_ATR_QUEUE_NAME, 1000, sizeof(struct ATR_qbuf));
    if (atr_msqid == -1) {
#ifdef APP_LAYER_DEBUG
    perror("appLayerInit:msgget.Error al crear la cola de respuestas AT Remotas");
#endif
        return -1;
    }

    nodes.node_list.next = 0;
    nodes.node_list.prev = 0;

    pthread_mutex_init(&node_list_mutex, NULL);

    uint8_t i = 0;
    while (_num_nodes < 1 && i < 5) {
        _num_nodes = nodeDiscovery();
        i++;
    }

    printNodes();

    return 0;
}

int appLayerExit() {

    if (pmsgq_destroy(APP_TX_STATUS_QUEUE_NAME, txs_msqid) == -1) {
#ifdef APP_LAYER_DEBUG
        perror(
                "appLayerExit:pmsgq_destroy.Error al destruir la cola de tx status");
#endif
        return -1;
    }

    if (pmsgq_destroy(APP_AT_QUEUE_NAME, at_msqid) == -1) {
#ifdef APP_LAYER_DEBUG
        perror(
                "appLayerExit:pmsgq_destroy.Error al destruir la cola de respuestas AT");
#endif
        return -1;
    }

    if (pmsgq_destroy(APP_ATR_QUEUE_NAME, atr_msqid) == -1) {
#ifdef APP_LAYER_DEBUG
        perror(
                "appLayerExit:pmsgq_destroy.Error al destruir la cola de respuestas AT Remotas");
#endif
        return -1;
    }

    wpan_node_t *tmp;
    struct list_head *pos, *q;

    if (!list_empty(&nodes.node_list)) {
        pthread_mutex_lock(&node_list_mutex);

        list_for_each_safe(pos, q, &nodes.node_list) {
            tmp = list_entry(pos, wpan_node_t, node_list);
            pmsgq_destroy(tmp->msq_name, tmp->msqid);
            list_del(pos);
            free(tmp);
        }

        pthread_mutex_unlock(&node_list_mutex);
    }

    if (xbeeSvcExit() == -1) {
#ifdef APP_LAYER_DEBUG
        perror("appLayerExit:xbeeSvcExit.Error al finalizar");
#endif
        return -1;
    }

    pthread_mutex_destroy(&node_list_mutex);
    return 0;

}

void appLayerInput(uint8_t mtype, void * packet) {

    struct ATR_qbuf atr_qbuf;
    struct AT_qbuf at_qbuf;
    struct TX_status_qbuf txs_qbuf;

    if (packet == NULL) {
#ifdef APP_LAYER_DEBUG
        printf("\n...");
#endif
    } else {

        switch (mtype) {
        case RX_16_RESPONSE: {
            xbee_RxFrame16_t *received16 = (xbee_RxFrame16_t*) packet;

            wpan_node_t *tmp = findNode(&nodes, received16->sourceAddress, 0);
            if (tmp != NULL) {
#ifdef APP_LAYER_DEBUG
                print_rx_16(*received16);
                printf("\nEste viene del Sr: %s\n", tmp->identifier);
#endif
                msgInput(tmp->msqid, tmp->identifier, received16->data,
                        received16->dataSize);

            } else {
                free(received16->data);
                received16 = NULL;
#ifdef APP_LAYER_DEBUG
                printf("\ndrop");
#endif
            }
            break;
        }
        case RX_64_RESPONSE: {
            xbee_RxFrame64_t *received64 = (xbee_RxFrame64_t*) packet;

            wpan_node_t *tmp = findNode(&nodes, 0, received64->sourceAddress);
            if (tmp != NULL) {
#ifdef APP_LAYER_DEBUG
                print_rx_64(*received64);
                printf("\nEste viene del Sr: %s\n", tmp->identifier);
#endif
                msgInput(tmp->msqid, tmp->identifier, received64->data,
                        received64->dataSize);
            } else {
                free(received64->data);
                received64 = NULL;
#ifdef APP_LAYER_DEBUG
                printf("\ndrop");
#endif
            }
            break;
        }
        case TX_STATUS_RESPONSE: {
            xbee_TxStatus_t *tx_response = (xbee_TxStatus_t *) packet;

            txs_qbuf.mtype = TX_STATUS_RESPONSE;
            txs_qbuf.tx_status = *(tx_response);
#ifdef APP_LAYER_DEBUG
            print_tx_status(*tx_response);
#endif

            if (pmsgq_send(txs_msqid, &txs_qbuf, sizeof(txs_qbuf),
                    TX_STATUS_RESPONSE, 1, 0) == -1) {
#ifdef APP_LAYER_DEBUG
                perror("appLayerInput:pmsgq_send.Error al colocar en la cola TXStatus");
#endif
            }
            break;
        }
        case AT_COMMAND_RESPONSE: {
            xbee_AtCommandResponse_t *response =
                    (xbee_AtCommandResponse_t*) packet;

            at_qbuf.mtype = AT_COMMAND_RESPONSE;
            at_qbuf.response = *(response);
#ifdef APP_LAYER_DEBUG
            print_AT_response(at_qbuf.response);
#endif

            if (pmsgq_send(at_msqid, &at_qbuf, sizeof(at_qbuf),
                    AT_COMMAND_RESPONSE, 1, 0) == -1) {
#ifdef APP_LAYER_DEBUG
                perror("appLayerInput:pmsgq_send.Error al colocar en la cola AT");
#endif
            }
            break;
        }
        case AT_REMOTE_RESPONSE: {
            xbee_AtRemoteResponse_t *response =
                    (xbee_AtRemoteResponse_t*) packet;

            atr_qbuf.mtype = AT_REMOTE_RESPONSE;
            atr_qbuf.response = *(response);
#ifdef APP_LAYER_DEBUG
            print_AT_remote_response(*response);
#endif

            if (pmsgq_send(atr_msqid, &atr_qbuf, sizeof(atr_qbuf),
                    AT_REMOTE_RESPONSE, 1, 0) == -1) {
#ifdef APP_LAYER_DEBUG
                perror("appLayerInput:pmsgq_send.Error al colocar en la cola ATRemote");
#endif
            }
            break;
        }
        default:
#ifdef APP_LAYER_DEBUG
            printf("\nother");
#endif
            break;
        }
    }
}

